#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy
from std_msgs.msg import UInt8
from sensor_msgs.msg import BatteryState
from diagnostic_msgs.msg import DiagnosticArray
from geometry_msgs.msg import TwistStamped
import sys
import termios
import tty

# 全局变量用于存储最新的消息
battery_msg = None
motor_msg = None
motion_msg = None
light_cmd = None

def battery_cb(msg):
    global battery_msg
    battery_msg = msg

def motor_cb(msg):
    global motor_msg
    motor_msg = msg

def motion_cb(msg):
    global motion_msg
    motion_msg = msg

def light_cb(msg):
    global light_cmd
    light_cmd = msg

def print_status():
    """打印所有状态信息"""
    # 使用 ANSI 转义序列清屏并移动光标到左上角
    sys.stdout.write("\x1b[2J\x1b[H")
    sys.stdout.flush()

    print("===== 松灵 Scout MINI 状态监控 (按 'q' 退出) =====")
    print("-" * 50)

    # 电池状态
    if battery_msg:
        voltage = "电压: {:.2f} V".format(battery_msg.voltage)
        percentage = " | 百分比: {:.1f}%".format(battery_msg.percentage * 100) if hasattr(battery_msg, 'percentage') and battery_msg.percentage == battery_msg.percentage else ""
        print("[电池] {}".format(voltage + percentage))
    else:
        print("[电池] 等待 /scout/battery 消息...")
    print("-" * 50)

    # 运动状态
    if motion_msg:
        print("[运动] 线速度: {:.3f} m/s | 角速度: {:.3f} rad/s".format(
            motion_msg.twist.linear.x, motion_msg.twist.angular.z))
    else:
        print("[运动] 等待 /scout/motion 消息...")
    print("-" * 50)

    # 电机状态
    if motor_msg and motor_msg.status:
        print("[电机]")
        for s in motor_msg.status:
            rpm_val = next((v.value for v in s.values if v.key == "rpm"), "N/A")
            cur_val = next((v.value for v in s.values if v.key == "current"), "N/A")
            tmp_val = next((v.value for v in s.values if v.key == "temperature"), "N/A")
            print("  - {:<12s} | RPM: {:<8s} | 电流: {:<8s} | 温度: {:<5s}".format(
                s.name, rpm_val, cur_val, tmp_val))
    else:
        print("[电机] 等待 /scout/motor_states 消息...")
    print("-" * 50)

    # 灯光指令
    if light_cmd:
        light_map = {0: "关闭", 1: "常亮", 2: "呼吸", 3: "自定义"}
        print("[灯光指令] 当前: {} ({})".format(light_cmd.data, light_map.get(light_cmd.data, "未知")))
    else:
        print("[灯光指令] 等待 /scout/light_cmd 消息...")
    print("-" * 50)


def main():
    rospy.init_node("scout_status_monitor", anonymous=True)

    rospy.Subscriber("/scout/battery", BatteryState, battery_cb)
    rospy.Subscriber("/scout/motor_states", DiagnosticArray, motor_cb)
    rospy.Subscriber("/scout/motion", TwistStamped, motion_cb)
    rospy.Subscriber("/scout/light_cmd", UInt8, light_cb)

    rate = rospy.Rate(5)  # 5Hz刷新率

    # 非阻塞式键盘输入设置
    old_settings = termios.tcgetattr(sys.stdin)
    tty.setcbreak(sys.stdin.fileno())

    print("启动监控... 按 'q' 键退出。")

    while not rospy.is_shutdown():
        print_status()
        # 检查键盘输入
        if sys.stdin in __import__("select").select([sys.stdin], [], [], 0)[0]:
            char = sys.stdin.read(1)
            if char == 'q':
                break
        rate.sleep()

    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old_settings)


if __name__ == "__main__":
    try:
        main()
    except rospy.ROSInterruptException:
        pass
    finally:
        # 恢复终端设置
        import termios, sys
        old_settings = termios.tcgetattr(sys.stdin)
        termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old_settings)
